/**
@author zhongyang
该模块是通过串口读取激光雷达的距离信息
串口号是用的、/dev/ttyUSB1，波特率是115200;正确设置串口信息才能读取，一般首先插入激光雷达的usb默认会是ttyUSB1.
发布的信息是h_distance,单位是m。即距离信息而非高度。
*/
#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h> 
#include <std_msgs/Float64.h>
#include <std_msgs/Empty.h> 
#include <stdio.h>
#include <stdlib.h>
#include <string>

using namespace std;

serial::Serial ser; //声明串口对象 

int main (int argc, char** argv) 
{ 
    //初始化节点 
    ros::init(argc, argv, "serial_example_node"); 
    //声明节点句柄 
    ros::NodeHandle nh; 

    //订阅主题，并配置回调函数 ,暂时没用到
    //ros::Subscriber write_sub = nh.subscribe("write", 1000, write_callback); 


    //发布主题 ，将激光雷达的距离信息发布出去
    ros::Publisher read_pub = nh.advertise<std_msgs::Float64>("h_distance", 50); 

    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB2"); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 

    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 
    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    } 

    //指定循环的频率 
    ros::Rate loop_rate(50); 

    while(ros::ok()) 
    { 
        if(ser.available()){ 
        ROS_INFO_STREAM("Reading from serial port\n"); 
        std_msgs::String result;
        std_msgs::Float64 result_pub;

        result.data = ser.read(ser.available());

        //去掉最后一个字符
        result.data.pop_back();
        //转换成flaot类型
        result_pub.data = atof(result.data.c_str());

        cout<<result_pub.data*100<<" "<<endl;

        read_pub.publish(result_pub); 
    } 

    //处理ROS的信息，比如订阅消息,并调用回调函数 
    ros::spinOnce(); 
    loop_rate.sleep(); 
    } 
    
    return 0;
} 
